import lejos.nxt.LCD;


public class LabyrinthTest {

	public static void main(String[] args) throws Exception {
		
		boolean trovato = false;
		
		int righelab = 16;
		int colonnelab = 12;
		
		LabyrinthSolver ls = new LabyrinthSolver(righelab,colonnelab);

		ls.calibra();

		ls.acquisisciMatrice();

		ls.convertiMatrice(ls.getLabirinto());

		/*########## RICORDA #########
		 * # X=SCANDISCE COLONNE     #
		 * # Y=SCANDISCE RIGHE       #
		 * ###########################
		 * */
		
		/* v2 dell'esplora con xylist
		 * 
		 * */
		
		if(ls.getLunghezzaList() >= 2){
			LCD.clear();
			LCD.drawString("Elaborazione...", 0, 1);
				if (ls.esplora(ls.getXYList().get(0).getX(), ls.getXYList().get(0).getY(), ls.getXYList().get(1).getX(), ls.getXYList().get(1).getY())) {
					LCD.clear();
					LCD.drawString("Percorso trovato", 0, 1);
					Thread.sleep(5000);
					trovato = true;
				} else {
					LCD.clear();
					LCD.drawString("Percorso non trovato!", 0, 1);
					Thread.sleep(5000);
					trovato = false;
				}
		}
		else{
			LCD.clear();
			LCD.drawString("Labirinto non risolvibile", 0, 1);
		}
		 		
		ls.riconvertiMatrice(ls.getLabirinto());

		ls.stampaMatriceInversa(ls.getLabirinto());

		Thread.sleep(2000);

		ls.riavvolgiMotore(ls.getAngle_y_rw(), ls.getRighe() + 1, ls.getSpeed_y_rw(), 0, ls.getMotore_y());
		
		if(trovato){			
			ls.posizionaPenna();
			ls.tracciaPercorso();
		}
	}
}
